/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 03:05:43
 * @Description: ...
 * @Reference: ...
 */

#include "kaist_publisher/encoder_converter.h"

namespace kaist_publisher {

EncoderConverter::EncoderConverter(ros::NodeHandle &nh,
                                   const std::string &dataset_dir,
                                   const std::string &save_dir,
                                   const std::string &topic)
    : Converter(dataset_dir, save_dir), topic_(topic) {
    bag_name_ = FilterSlash(topic_) + ".bag";
    pub_encoder_ = nh.advertise<lci_slam::encoder>(topic_.c_str(), 100);
}

int64_t EncoderConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_, "%ld,%ld,%ld\n", &stamp_, &left_count_, &right_count_) ==
        3) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t EncoderConverter::Publish() {
    lci_slam::encoder encoder_data;
    encoder_data.header.stamp.fromNSec(stamp_);
    encoder_data.header.frame_id = "encoder";
    encoder_data.left_count = left_count_;
    encoder_data.right_count = right_count_;
    pub_encoder_.publish(encoder_data);

    if (fscanf(fp_, "%ld,%ld,%ld\n", &stamp_, &left_count_, &right_count_) ==
        3) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void EncoderConverter::Publish(int64_t &timestamp,
                               lci_slam::encoderPtr &encoder_data) {
    encoder_data.reset(new lci_slam::encoder());
    encoder_data->header.stamp.fromNSec(stamp_);
    encoder_data->header.frame_id = "encoder";
    encoder_data->left_count = left_count_;
    encoder_data->right_count = right_count_;
    // pub_encoder_.publish(encoder_data);

    if (fscanf(fp_, "%ld,%ld,%ld\n", &stamp_, &left_count_, &right_count_) ==
        3) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int EncoderConverter::Convert() {
    CheckAndCreateSaveDir();
    boost::filesystem::path bag_file =
        boost::filesystem::path(save_dir_) / bag_name_;
    // rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE *fp = fopen(data_file.c_str(), "r");
    lci_slam::encoder encoder_data;
    int64_t stamp, left_count, right_count;
    while (fscanf(fp, "%ld,%ld,%ld\n", &stamp, &left_count, &right_count) ==
           3) {
        encoder_data.header.stamp.fromNSec(stamp);
        encoder_data.header.frame_id = "encoder";
        encoder_data.left_count = left_count;
        encoder_data.right_count = right_count;

        // bag.write(topic_, encoder_data.header.stamp, encoder_data);
        pub_encoder_.publish(encoder_data);
    }
    // bag.close();
    // fclose(fp);
    // ROS_INFO("done saving %s", bag_file.c_str());

    return 0;
}
}  // namespace kaist_publisher
